#!/usr/bin/python
'''
Created on Jul 24, 2011

@author: Alessandro Stranieri
'''

import os
import sys
import random
import logging
import math
import subprocess
import xml.etree.cElementTree as etree

##################
### PARAMETERS ###
##################

# Random seed
rseed = 666

# List of the robot IDs involved in the experiment 
robot_ids = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10]

# Experiment single phase length in simulation time steps
exp_length = 10000

# ARGoS installation root directory
argos_install_dir = os.environ.get("ARGOSINSTALLDIR")

# Path to the template experiment configuration file
template_xml_file_name = argos_install_dir + "/user/eliseofe/xml/flocking/flocking_motion_control.xml"

# Destination path of experiment xml configuration files
local_destination_directory_name = "output/"

# Destination of configuration file on the robot
remote_destination_directory_name = "eliseofe/"

##############
### SET-UP ###
##############

# LOGGING
logger = logging.getLogger("GenerateFlockingExperimentXML")
logger.setLevel(logging.DEBUG)
ch = logging.StreamHandler(sys.stdout)
ch.setLevel(logging.DEBUG)
formatter = logging.Formatter("[%(levelname)s | %(name)s] %(message)s")
ch.setFormatter(formatter)
logger.addHandler(ch)

# Check that template file exists
logger.info("Checking existence of template file <%s>" % template_xml_file_name )
if os.path.exists(template_xml_file_name) :
    logger.info("XML template found: " + template_xml_file_name)
else:
    logger.error("XML template not found. Exiting...")
    exit(1)
    
# Setting-up destination directory
logger.info("Checking existence of destination directory <%s>" % (local_destination_directory_name))
if os.path.isdir(local_destination_directory_name):
    logger.info("XML destination directory found.")
else:
    logger.error("XML destination directory found. Exiting...")
    exit(1)


# Set random seed
random.seed(rseed)

# Choose informed robot
random.shuffle(robot_ids)
logger.debug("Robot list after shuffling: %s" % (robot_ids))
informed_robot_id_1 = robot_ids.pop()
logger.info("Informed robot 1 is FB%02d" % (informed_robot_id_1))

# Choose second informed robot
random.shuffle(robot_ids)
logger.debug("Robot list after shuffling: %s" % (robot_ids))
informed_robot_id_2 = robot_ids.pop()
logger.info("Informed robot 2 is FB%02d" % (informed_robot_id_2))

robot_ids.extend([informed_robot_id_1, informed_robot_id_2])
robot_ids.sort()

# Choose goal direction
g1 = random.uniform(0, 2*math.pi)
logger.info("Goal direction 1 is %.2f rad (%.1f deg)" % (g1, math.degrees(g1)))
g2 = g1 - math.copysign(math.pi, (g1-math.pi))
logger.info("Goal direction 2 is %.2f rad (%.1f deg)" % (g2, math.degrees(g2)))
goal_direction_list = []

g0_str = str(-1)
g1_str = "%.2f" % (g1)
g2_str = "%.2f" % (g2)
for robot_id in robot_ids:
    robot_goal_directions = []
    
    if robot_id == informed_robot_id_1:
        robot_goal_directions.extend([g1_str, g1_str, g1_str, g1_str])
    elif robot_id == informed_robot_id_2:
        robot_goal_directions.extend([g0_str, g2_str, g2_str, g0_str])
    else:
        robot_goal_directions.extend([g0_str, g0_str, g0_str, g0_str])
    
    goal_direction_list.append(robot_goal_directions)


# Open and parse template file
xml_tree = etree.parse(template_xml_file_name)
xml_root = xml_tree.getroot()

# Get <controllers> element
xml_controllers = xml_root.find("controllers")
# Get <parameters> element
xml_bt_footbot_flocking_controller = xml_controllers.find("bt_footbot_flocking_controller")
xml_parameters = xml_bt_footbot_flocking_controller.find("parameters")
# Create element <real_robot>
real_robot_element = etree.Element("real_robot")
real_robot_element.set("active", "true")
xml_parameters.append(real_robot_element)

# Loop through robot IDs
for robot_id in robot_ids:
    
    logger.info("Configuring footbot FB%02d" % (robot_id))

    # Set real_robot attributes
    real_robot_element.set("phase_1", goal_direction_list[robot_id-1][0])
    real_robot_element.set("phase_2", goal_direction_list[robot_id-1][1])
    real_robot_element.set("phase_3", goal_direction_list[robot_id-1][2])
    real_robot_element.set("phase_4", goal_direction_list[robot_id-1][3])

    # Write configuration file
    xml_configuration_file_name = "flocking_FB%02d.xml" % (robot_id)
    local_xml_configuration_path = "%s%s" % (local_destination_directory_name, xml_configuration_file_name)
    logger.info("Writing configuration to: <%s>" % (xml_configuration_file_name))
    xml_tree.write(local_xml_configuration_path, encoding="utf-8")

    #########################
    ### Copy XML to robot ###
    #########################
    #host = "root@10.0.0.1%02d" % (robot_id)
    host = "jdestefani@fallopius"
    logger.info("Check existence of destination directory on host %s" % (host))
    retcode = subprocess.call(["ssh", host, "cd", remote_destination_directory_name], stdout=sys.stdout, stderr=sys.stderr)
    
    if(retcode == 0):
        logger.info("Destination exists. Copying...")
        remote_xml_configuration_path = "%s%s" % (remote_destination_directory_name, xml_configuration_file_name)
        destination = "%s:%s" % (host, remote_xml_configuration_path)
        retcode = subprocess.call(["scp", local_xml_configuration_path, destination], stdout=sys.stdout, stderr=sys.stderr)
        
        if(retcode > 0):
            logger.warning("Problem during copy. Return code %d" % (retcode))
        
    else:
        logger.warning("Destination doesn't exist. Skipping...")

################
### FINISHED ###
################
logger.info("Exiting.")